

#include "erl_nif.h"
#include <string.h>

#include <stdio.h>
#include <ctype.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include <map>

#include "navmesh.h"

#define EXCLUDE_MAX 0xffff

static ERL_NIF_TERM atom_ok;
static ERL_NIF_TERM atom_error;
static ERL_NIF_TERM atom_true;
static ERL_NIF_TERM atom_false;
static ERL_NIF_TERM atom_undefined;
static ERL_NIF_TERM atom_hit;
static ERL_NIF_TERM atom_unhit;


struct x_nav_priv_data
{
    std::map<int, Navmesh*>* meshes;
    ErlNifMutex* lock;
};

static int load(ErlNifEnv* env, void** priv, ERL_NIF_TERM load_info)
{
    x_nav_priv_data* data = (x_nav_priv_data*)enif_alloc(sizeof(x_nav_priv_data));
    if (NULL == data)
    {
        return 1;
    }

    data->meshes = new std::map<int, Navmesh*>;
    data->lock = enif_mutex_create((char*)"x_nav_lock");

    if (NULL == data->meshes || NULL == data->lock)
    {
        return 2;
    }

    *priv = data;

    atom_ok = enif_make_atom(env, "ok");
    atom_error = enif_make_atom(env, "error");
    atom_true = enif_make_atom(env, "true");
    atom_false = enif_make_atom(env, "false");
    atom_undefined = enif_make_atom(env, "undefined");
    atom_hit = enif_make_atom(env, "hit");
    atom_unhit = enif_make_atom(env, "unhit");

    return 0;
}

static int upgrade(ErlNifEnv* env, void** priv_data, void** old_priv_data, ERL_NIF_TERM load_info)
{
    if (NULL == *old_priv_data)
    {
        return 1;
    }
    *priv_data = *old_priv_data;
    return 0;
}


ERL_NIF_TERM make_point(ErlNifEnv* env, VECTOR3 vec)
{
    return enif_make_tuple3(env, enif_make_double(env, vec.x),
        enif_make_double(env, vec.y), enif_make_double(env, vec.z));
}

bool get_number(ErlNifEnv* env, const ERL_NIF_TERM term, double* num)
{
    int t;
    if (enif_get_double(env, term, num))
    {
        return true;
    }
    else
    {
        if (enif_get_int(env, term, &t))
        {
            *num = (double)t;
            return true;
        }
        else
        {
            return false;
        }
    }
}

bool get_point(ErlNifEnv* env, const ERL_NIF_TERM* tuple3, VECTOR3* vec)
{
    int t;
    double pt[3];

    for (int i=0; i<3; i++)
    {
        if (!enif_get_double(env, *tuple3, &pt[i]))
        {
            if (enif_get_int(env, *tuple3, &t))
            {
                pt[i] = (double)t;
            }
            else
            {
                return false;
            }
        }
        tuple3++;
    }
    vec->x = pt[0];
    vec->y = pt[1];
    vec->z = pt[2];
    return true;
}

std::map<int, Navmesh*>* get_navmeshes(ErlNifEnv* env)
{
    return ((x_nav_priv_data*)enif_priv_data(env))->meshes;
}

ErlNifMutex* get_priv_lock(ErlNifEnv* env)
{
    return ((x_nav_priv_data*)enif_priv_data(env))->lock;
}

Navmesh* get_navmesh(ErlNifEnv* env, int mapid)
{
    std::map<int, Navmesh*>* meshes;
    std::map<int, Navmesh*>::iterator navm_it;

    meshes = get_navmeshes(env),
    navm_it = meshes -> find(mapid);
    if (navm_it == meshes->end())
    {
        return NULL;
    }
    else
    {
        return navm_it->second;
    }
}

static ERL_NIF_TERM load_map(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    ErlNifBinary pathbin;
    ErlNifMutex* lock;

    if (!enif_get_int(env, argv[0], &mapid)
        || !enif_inspect_iolist_as_binary(env, argv[1], &pathbin))
    {
        return enif_make_badarg(env);
    }

    lock = get_priv_lock(env);
    enif_mutex_lock(lock);

    std::map<int, Navmesh*>* meshes = get_navmeshes(env);
    if (meshes->find(mapid) != meshes->end())
    {
        enif_mutex_unlock(lock);
        return enif_make_tuple2(env, enif_make_atom(env, "error"), enif_make_atom(env, "mapid_exist"));
    }

    Navmesh* navm;

    char* pstr = (char*)enif_alloc(pathbin.size+1);
    memcpy(pstr, pathbin.data, pathbin.size);
    *(pstr + pathbin.size) = '\0';

    navm = new Navmesh(pstr);

    enif_free(pstr);

    if (NULL == navm)
    {
        enif_mutex_unlock(lock);
        return enif_make_tuple2(env, enif_make_atom(env, "error"), enif_make_atom(env, "load_error"));
    }
    else if (!navm->is_good())
    {
        int err = navm->get_errno();
        delete navm;
        enif_mutex_unlock(lock);
        return enif_make_tuple2(env, enif_make_atom(env, "error"), enif_make_int(env, err));
    }

    meshes->insert(std::pair<int, Navmesh*>(mapid, navm));
    enif_mutex_unlock(lock);

    return atom_ok;
}


static ERL_NIF_TERM has_map(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;

    if (!enif_get_int(env, argv[0], &mapid))
    {
        return enif_make_badarg(env);
    }

    if (NULL == get_navmesh(env, mapid))
    {
        return atom_false;
    }
    else
    {
        return atom_true;
    }
}


static ERL_NIF_TERM maps(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    std::map<int, Navmesh*>* meshes;
    std::map<int, Navmesh*>::reverse_iterator navm_it;

    meshes = get_navmeshes(env);

    ERL_NIF_TERM maps;
    maps = enif_make_list(env, 0);

    for(navm_it = meshes->rbegin(); navm_it != meshes->rend(); navm_it++)
    {
        maps = enif_make_list_cell(env, enif_make_int(env, navm_it->first), maps);
    }
    return maps;
}


static ERL_NIF_TERM random_pos_over_map(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    unsigned int exclude;

    if (!enif_get_int(env, argv[0], &mapid))
    {
        return enif_make_badarg(env);
    }

    if (!enif_get_uint(env, argv[1], &exclude) || exclude > EXCLUDE_MAX)
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    VECTOR3 vec;
    if (navm -> find_random_pos_over_map(&vec, exclude))
    {
        return make_point(env, vec);
    }
    else
    {
        return atom_undefined;
    }
}

static ERL_NIF_TERM get_border(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    ERL_NIF_TERM bmin, bmax;

    if (!enif_get_int(env, argv[0], &mapid))
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    bmin = make_point(env, navm->border_min);
    bmax = make_point(env, navm->border_max);

    return enif_make_tuple2(env, bmin, bmax);
}

static ERL_NIF_TERM random_pos(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    const ERL_NIF_TERM *pos;
    int pos_n = 0;
    double radius = 0.0;
    unsigned int exclude;

    if ((!enif_get_int(env, argv[0], &mapid))
        || (!enif_get_tuple(env, argv[1], &pos_n, &pos))
        || (!get_number(env, argv[2], &radius))
        || pos_n != 3
        || radius < 0)
    {
        return enif_make_badarg(env);
    }

    if (!enif_get_uint(env, argv[3], &exclude) || exclude > EXCLUDE_MAX)
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    VECTOR3 vec;
    if (!get_point(env, pos, &vec))
    {
        return enif_make_badarg(env);
    }

    VECTOR3 vec2;
    if (navm -> find_random_pos(&vec, radius, &vec2, exclude))
    {
        return make_point(env, vec2);
    }
    else
    {
        return atom_undefined;
    }
}


static ERL_NIF_TERM is_walkable(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    const ERL_NIF_TERM *pos;
    int pos_n = 0;
    unsigned int exclude;

    if ((!enif_get_int(env, argv[0], &mapid))
        || (!enif_get_tuple(env, argv[1], &pos_n, &pos))
        || pos_n != 3)
    {
        return enif_make_badarg(env);
    }

    if (!enif_get_uint(env, argv[2], &exclude) || exclude > EXCLUDE_MAX)
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    VECTOR3 vec;
    if (!get_point(env, pos, &vec))
    {
        return enif_make_badarg(env);
    }

    VECTOR3 vec2;
    if (navm -> find_reasonal_pos(&vec, 0.002f, &vec2, exclude))
    {
        vec.y = vec2.y;
        navm -> find_reasonal_pos(&vec, 0.002f, &vec2, exclude);
        if (fabs(vec2.x - vec.x) <= 0.002 && fabs(vec2.z - vec.z) <= 0.002)
        {
            return atom_true;
        }
        else
        {
            return atom_false;
        }
    }
    else
    {
        return atom_false;
    }
}


static ERL_NIF_TERM reasonal_pos(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    double radius;
    const ERL_NIF_TERM *pos;
    int pos_n = 0;
    unsigned int exclude;

    if ((!enif_get_int(env, argv[0], &mapid))
        || (!enif_get_tuple(env, argv[1], &pos_n, &pos))
        || (!get_number(env, argv[2], &radius))
        || pos_n != 3)
    {
        return enif_make_badarg(env);
    }

    if (!enif_get_uint(env, argv[3], &exclude) || exclude > EXCLUDE_MAX)
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    VECTOR3 vec;
    if (!get_point(env, pos, &vec))
    {
        return enif_make_badarg(env);
    }

    VECTOR3 vec2;
    if (navm -> find_reasonal_pos(&vec, radius, &vec2, exclude))
    {
        return make_point(env, vec2);
    }
    else
    {
        return atom_undefined;
    }
}

static ERL_NIF_TERM raycast(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    const ERL_NIF_TERM *spos, *epos;
    int spos_n = 0;
    int epos_n = 0;
    unsigned int exclude;

    if ((!enif_get_int(env, argv[0], &mapid))
        || (!enif_get_tuple(env, argv[1], &spos_n, &spos))
        || (!enif_get_tuple(env, argv[2], &epos_n, &epos))
        || spos_n != 3
        || epos_n != 3)
    {
        return enif_make_badarg(env);
    }

    if (!enif_get_uint(env, argv[3], &exclude) || exclude > EXCLUDE_MAX)
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    VECTOR3 s_vec;
    VECTOR3 e_vec;
    if (!get_point(env, spos, &s_vec)
        || !get_point(env, epos, &e_vec))
    {
        return enif_make_badarg(env);
    }

    VECTOR3 hit_vec;
    float rate;
    if (!navm->raycast(&s_vec, &e_vec, &hit_vec, &rate, exclude))
    {
        return atom_undefined; // 起点不可行走
    }

    if (rate < 1.0)
    {
        return enif_make_tuple2(env, atom_hit, make_point(env, hit_vec));
    }
    else
    {
        return enif_make_tuple2(env, atom_unhit, make_point(env, hit_vec));
    }
}


static ERL_NIF_TERM find_path(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    const ERL_NIF_TERM *spos, *epos;
    int spos_n = 0;
    int epos_n = 0;
    unsigned int exclude;

    if ((!enif_get_int(env, argv[0], &mapid))
        || (!enif_get_tuple(env, argv[1], &spos_n, &spos))
        || (!enif_get_tuple(env, argv[2], &epos_n, &epos))
        || spos_n != 3
        || epos_n != 3)
    {
        return enif_make_badarg(env);
    }

    if (!enif_get_uint(env, argv[3], &exclude) || exclude > EXCLUDE_MAX)
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    VECTOR3 s_vec;
    VECTOR3 e_vec;
    if (!get_point(env, spos, &s_vec)
        || !get_point(env, epos, &e_vec))
    {
        return enif_make_badarg(env);
    }

    std::vector<VECTOR3> PathPoint;

    if (!navm->findpath(&s_vec, &e_vec, &PathPoint, exclude))
    {
        return atom_undefined;
    }

    ERL_NIF_TERM ret_path = enif_make_list(env, 0);
    std::vector<VECTOR3>::reverse_iterator rit = PathPoint.rbegin();
    for (; rit!= PathPoint.rend(); ++rit)
    {
        ret_path = enif_make_list_cell(env, make_point(env, *rit), ret_path);
    }
    return ret_path;
}


static ERL_NIF_TERM move_along_surface(ErlNifEnv* env, int argc, const ERL_NIF_TERM argv[])
{
    int mapid;
    const ERL_NIF_TERM *spos, *epos;
    int spos_n = 0;
    int epos_n = 0;

    if ((!enif_get_int(env, argv[0], &mapid))
        || (!enif_get_tuple(env, argv[1], &spos_n, &spos))
        || (!enif_get_tuple(env, argv[2], &epos_n, &epos))
        || spos_n != 3
        || epos_n != 3)
    {
        return enif_make_badarg(env);
    }

    Navmesh* navm = get_navmesh(env, mapid);
    if (NULL == navm)
    {
        return enif_make_badarg(env);
    }

    VECTOR3 s_vec;
    VECTOR3 e_vec;
    if (!get_point(env, spos, &s_vec)
        || !get_point(env, epos, &e_vec))
    {
        return enif_make_badarg(env);
    }

    VECTOR3 target;

    if (!navm->move_along_surface(&s_vec, &e_vec, &target))
    {
        return atom_undefined;
    }

    return make_point(env, target);
}

static ErlNifFunc nif_funcs[] =
{
    {"load_map", 2, load_map},
    {"has_map", 1, has_map},
    {"get_border", 1, get_border},
    {"maps", 0, maps},
    {"find_path", 4, find_path},
    {"is_walkable", 3, is_walkable},
    {"reasonal_pos", 4, reasonal_pos},
    {"raycast", 4, raycast}
};

ERL_NIF_INIT(x_nav, nif_funcs, load, NULL, upgrade, NULL)

